#include "can_func.h"
#include "dji_ctr.h"
#ifdef SL_CAN

#ifdef SL_DEBUG
#include "cmd.h"
#endif // SL_DEBUG
// #include "mtr_driver.h"

void can_get_mag_mtr(can_msg *data);

//添加接收canId
void can_func_init() {
    #ifdef SL_DEBUG
    can_callback_add(1, can_suc_rx);
    can_callback_add(325, can_show_button);
    can_callback_add(324, can_show_rocker);
    #endif
    can_callback_add(0x281, can_read_motor);
}
    
//增加接收函数
void can_read_motor(can_msg *data)
{
    int id = (data->in[0])>>16;
    int index = ((data->in[0])<<16)>>16; 
    
    if(index == 0x00005850){
        //位置信息
        int num = data->in[1];
        Motor[id].pos = num;
        // uprintf("motor %d's pos =%d\r\n",id,num);
        
    }
    else if(index == 0x00005856){
        //速度信息
        int num = data->in[1];
        Motor[id].vel = num;
        uprintf("motor %d's vel =%d\r\n",id,num/1000);
    }
    else if(index == 0x00005149){
        //电流信息
        float num = data->fl[1];
        Motor[id].cur = (int)num;
        uprintf("motor %d's cur =%f\r\n",id,num);
    }
}
__weak void can_rx_callback(can_msg *data) {}

#ifdef SL_DEBUG
void can_suc_rx(can_msg *data) {
    uprintf("can rx ok\r\n");
}

void can_show_button(can_msg *data) {
    uprintf("%d %d %c\r\n", data->ui8[0], data->ui8[1], data->ui8[2]);
}

int can_data_show_flag = 0;
void can_show_rocker(can_msg *data) {
    if (can_data_show_flag) {
        uprintf("%4d %4d %4d %4d\r", (int16_t)data->ui16[0], 
                                (int16_t)data->ui16[1],
                                (int16_t)data->ui16[2],
                                (int16_t)data->ui16[3]);
    }
}
#endif // SL_DEBUG

#endif // SL_CAN